#include <opencv2/opencv.hpp>
#include <iostream>
#include <cstdlib>
#include <ctime>   
#include <random>
#include "kalman.h"

using namespace cv;
using namespace std;

int main() {
    // 创建窗口
    namedWindow("Ball Animation", WINDOW_AUTOSIZE);
    kf::KalmanFilter KF(4,4,0,1.0);
    Matrix4d F;
    F<<1,0,1,0,
        0,1,0,1,
        0,0,1,0,
        0,0,0,1;
    Matrix4d H;
    H<<1,0,0,0,
        0,1,0,0,
        0,0,1,0,
        0,0,0,1;
    Matrix4d Q = Matrix4d::Identity()*1e-5;
    Matrix4d R = Matrix4d::Identity()*1e-2;
    KF.setTansMat(F);
    KF.setMeasMat(H);
    KF.setQ(Q);
    KF.setR(R);

    // 初始化小球的参数
    const int ballRadius = 15; // 小球半径
    Point ballCenter(0, 150); // 小球初始位置
    int dx = 30, dy = 15; // 小球的水平和垂直速度，用于生成轨迹
    int noiseX = 0, noiseY = 0; // 随机噪声
    // 用于存储小球的轨迹点
    vector<Point> trajectory;
    vector<Point> preTrajectory;
    //传感器测量的小球状态
    Point lastCenter(0, 0); // 小球上一时刻位置
    Point v(0, 0); // 小球速度
    Vector4d measS;

    // 循环直到用户关闭窗口
    while (true) {
        // 创建一个与窗口大小相同的黑色图像
        Mat frame = Mat::zeros( Size(1280, 640), CV_8UC3 );
        KF.predict();

        // 为小球的移动添加随机噪声
        
        noiseX = rand() % 3 - 1; // 生成-1, 0, 1的随机数
        noiseY = rand() % 65 - 32;  // 生成-32, 32的随机数

        // 绘制小球
        circle(frame, ballCenter, ballRadius, Scalar(0, 255, 0), 2);

        // 绘制轨迹
        polylines(frame, trajectory, false, Scalar(0, 255, 0),2);
        polylines(frame, preTrajectory, false, Scalar(255, 255, 0),2);

        //传感器得到的小球的状态
        v = ballCenter - lastCenter;
        lastCenter = ballCenter;
        measS<<ballCenter.x,ballCenter.y,v.x,v.y;
        //修正kalman
        MatrixXd preState= KF.correct(measS);
        Point predictCenter = Point(preState(0,0),preState(1,0));
        cout<<predictCenter;
        // 绘制小球
        circle(frame, predictCenter, 8, Scalar(255, 255, 0), 4);

        // 显示图像
        imshow("Ball Animation", frame);

        // 等待键盘事件，延时40ms
        if (waitKey(40) >= 0) break;

        // 更新小球位置
        ballCenter.x += dx +noiseX;
        ballCenter.y = 150 + dy + noiseY;

        // 检查小球是否移出窗口边界
        if (ballCenter.x - ballRadius > frame.cols || ballCenter.x + ballRadius < 0 ||
            ballCenter.y - ballRadius > frame.rows || ballCenter.y + ballRadius < 0) {
            ballCenter.x -= dx; // 如果移出边界，将小球位置回退
            ballCenter.y -= dy;
            dx = -dx; // 改变小球的水平速度
            dy = -dy; // 改变小球的垂直速度
        }

        // 将当前位置添加到轨迹中
        trajectory.push_back(ballCenter);

        // 限制轨迹点的数量，避免内存溢出
        if (trajectory.size() > 20) {
            trajectory.erase(trajectory.begin()); 
        }

        preTrajectory.push_back(predictCenter);

        // 限制轨迹点的数量，避免内存溢出
        if (preTrajectory.size() > 50) {
            preTrajectory.erase(preTrajectory.begin()); 
        }
        
    }

    return 0;
}